#include "ros/ros.h"
#include "std_msgs/String.h"
#include "mbot_linux_serial.h"

// 定义变量以接收 readSpeed 函数的输出
unsigned char checkFlag = 0x00;
unsigned char statusInfo = 0x00;
unsigned char errorInfo = 0x00;
int speedValues[5] = {0};
int positionValues[5] = {0};

void readSpeedAndDisplay() {
    // 调用 readSpeed 函数并检查返回的布尔值
    bool success = readSpeed(checkFlag, statusInfo, errorInfo, speedValues, positionValues);

    // if (success) {
    //     // 打印读取到的数据
    //     ROS_INFO("Check Flag: 0x%02X, Status Info: 0x%02X, Error Info: 0x%02X ", 
    //              checkFlag, statusInfo, errorInfo);

    //     ROS_INFO("Speed Values: %d, %d, %d, %d, %d", 
    //              speedValues[0], speedValues[1], speedValues[2], speedValues[3], speedValues[4]);

    //     ROS_INFO("Position Values: %d, %d, %d, %d, %d", 
    //              positionValues[0], positionValues[1], positionValues[2], positionValues[3], positionValues[4]);
    // } else {
    //     ROS_WARN("Failed to read data.");
    // }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "read_speed_node");
    ros::NodeHandle nh;

    Serial_Init(); // 串口初始化

    ros::Rate loop_rate(33.33); // 设置循环频率

    while (ros::ok()) {
        readSpeedAndDisplay(); // 读取速度并显示数据
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
